/*
 * Blobfinder.java
 *
 * Created on 17 de febrero de 2008, 12:48 AM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Robot.Sensors;

import Robot.*;
import javaclient3.*;
import java.util.ArrayList;
import javaclient3.structures.PlayerConstants;
import javaclient3.structures.blobfinder.*;
import javaclient3.BlobfinderInterface;
import Utils.*;
import java.util.Vector;
/**
 *
 * @author alejo
 */
public class Blobfinder extends Sensor implements Vector2DArraySensor{
    
    BlobfinderInterface BFDev;
    int numIRs;
    
    
    Vector2d[] data;
    ColorBlob obstColor;
    
    PlayerBlobfinderData dataBF;
    
    PlayerBlobfinderBlob[] blobs;
    int blobsCount;
    
    int minArea, maxArea;
    double[] transMatrix = new double[9];
    double[] intrinsicParams = new double[9];
    double[] radialDistParams = new double[2];
    
    /** Creates a new instance of IR */
    public Blobfinder(Robot robot, String name, int index) throws Exception{

        super(robot, name, index);

        BFDev=robot.requestInterfaceBlobfinder(deviceIndex, PlayerConstants.PLAYER_OPEN_MODE);
        
        if(BFDev==null)
               throw new Exception("Error requesting interface Blobfinder!");
        else
            this.setPlayerDevice(BFDev);
    
        
        data = new Vector2d[1];
        data[0] = new Vector2d(0,0,Vector2d.MODE_XY);
        
    }
    
     public Vector2d[] read(int mode) {
        
        //if READ_MODE_WAIT it will waits until new data is available
        //else, returns the old data
        switch(mode)
        {
            case READ_MODE_WAIT:
                while(!update());
            break;
            case READ_MODE_NO_WAIT:
                update();
            break;
        
        }
        return data; 
    }
     
    public boolean update(){
        
        if(isReady()) {
            
            //get data from blobfinder
            dataBF = BFDev.getData();
            blobs = dataBF.getBlobs();
            blobsCount = dataBF.getBlobs_count();
            
            //buffer for vectors to obstacles
            //TODO: para pruebas se definirán todos los blobs como obstáculos, INCLUYENDO
            //los blobs que conforman a los robots.
            Vector dataTemp = new Vector(1,1);
            //data = new Vector2d[blobsCount];
            //System.out.println("Number of blobs: "+blobsCount);
            int dataCount=0;
            for(int i=0;i<blobsCount;i++)
            {
                
            }
            return true;
        }
        else
            return false;
    }
    
    public boolean isReady()
    {
        return BFDev.isDataReady();
    }
    

    
}

